#include "../camera_models/Camera.h"
#include "../camera_models/ScaramuzzaCamera.h"

#include <opencv2/calib3d/calib3d.hpp>

namespace camodocal {

Camera::Parameters::Parameters(ModelType modelType):
  m_modelType(modelType),
  m_imageWidth(0),
  m_imageHeight(0) {
  switch(modelType) {
  case KANNALA_BRANDT:
    m_nIntrinsics = 8;
    break;
  case PINHOLE:
    m_nIntrinsics = 8;
    break;
  case SCARAMUZZA:
    m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
    break;
  case MEI:
  default:
    m_nIntrinsics = 9;
  }
}

Camera::Parameters::Parameters(ModelType          modelType,
                               const std::string& cameraName,
                               int                w,
                               int                h):
  m_modelType(modelType),
  m_cameraName(cameraName),
  m_imageWidth(w),
  m_imageHeight(h) {
  switch(modelType) {
  case KANNALA_BRANDT:
    m_nIntrinsics = 8;
    break;
  case PINHOLE:
    m_nIntrinsics = 8;
    break;
  case SCARAMUZZA:
    m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
    break;
  case MEI:
  default:
    m_nIntrinsics = 9;
  }
}

Camera::ModelType&
Camera::Parameters::modelType(void) {
  return m_modelType;
}

std::string&
Camera::Parameters::cameraName(void) {
  return m_cameraName;
}

int& Camera::Parameters::imageWidth(void) {
  return m_imageWidth;
}

int& Camera::Parameters::imageHeight(void) {
  return m_imageHeight;
}

Camera::ModelType
Camera::Parameters::modelType(void) const {
  return m_modelType;
}

const std::string&
Camera::Parameters::cameraName(void) const {
  return m_cameraName;
}

int Camera::Parameters::imageWidth(void) const {
  return m_imageWidth;
}

int Camera::Parameters::imageHeight(void) const {
  return m_imageHeight;
}

int Camera::Parameters::nIntrinsics(void) const {
  return m_nIntrinsics;
}

cv::Mat&
Camera::mask(void) {
  return m_mask;
}

const cv::Mat&
Camera::mask(void) const {
  return m_mask;
}

void Camera::estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,
                                const std::vector<cv::Point2f>& imagePoints,
                                cv::Mat&                        rvec,
                                cv::Mat&                        tvec) const {
  std::vector<cv::Point2f> Ms(imagePoints.size());
  for(size_t i = 0; i < Ms.size(); ++i) {
    Eigen::Vector3d P;
    liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);

    P /= P(2);

    Ms.at(i).x = P(0);
    Ms.at(i).y = P(1);
  }

  // assume unit focal length, zero principal point, and zero distortion
  cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
}

double
Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const {
  Eigen::Vector2d p1, p2;

  spaceToPlane(P1, p1);
  spaceToPlane(P2, p2);

  return (p1 - p2).norm();
}

double
Camera::reprojectionError(const std::vector<std::vector<cv::Point3f>>& objectPoints,
                          const std::vector<std::vector<cv::Point2f>>& imagePoints,
                          const std::vector<cv::Mat>&                  rvecs,
                          const std::vector<cv::Mat>&                  tvecs,
                          cv::OutputArray                              _perViewErrors) const {
  int    imageCount  = objectPoints.size();
  size_t pointsSoFar = 0;
  double totalErr    = 0.0;

  bool    computePerViewErrors = _perViewErrors.needed();
  cv::Mat perViewErrors;
  if(computePerViewErrors) {
    _perViewErrors.create(imageCount, 1, CV_64F);
    perViewErrors = _perViewErrors.getMat();
  }

  for(int i = 0; i < imageCount; ++i) {
    size_t pointCount = imagePoints.at(i).size();

    pointsSoFar += pointCount;

    std::vector<cv::Point2f> estImagePoints;
    projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints);

    double err = 0.0;
    for(size_t j = 0; j < imagePoints.at(i).size(); ++j) {
      err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));
    }

    if(computePerViewErrors) {
      perViewErrors.at<double>(i) = err / pointCount;
    }

    totalErr += err;
  }

  return totalErr / pointsSoFar;
}

double
Camera::reprojectionError(const Eigen::Vector3d&    P,
                          const Eigen::Quaterniond& camera_q,
                          const Eigen::Vector3d&    camera_t,
                          const Eigen::Vector2d&    observed_p) const {
  Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;

  Eigen::Vector2d p;
  spaceToPlane(P_cam, p);

  return (p - observed_p).norm();
}

void Camera::projectPoints(const std::vector<cv::Point3f>& objectPoints,
                           const cv::Mat&                  rvec,
                           const cv::Mat&                  tvec,
                           std::vector<cv::Point2f>&       imagePoints) const {
  // project 3D object points to the image plane
  imagePoints.reserve(objectPoints.size());

  //double
  cv::Mat R0;
  cv::Rodrigues(rvec, R0);

  Eigen::MatrixXd R(3, 3);
  R << R0.at<double>(0, 0), R0.at<double>(0, 1), R0.at<double>(0, 2),
    R0.at<double>(1, 0), R0.at<double>(1, 1), R0.at<double>(1, 2),
    R0.at<double>(2, 0), R0.at<double>(2, 1), R0.at<double>(2, 2);

  Eigen::Vector3d t;
  t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);

  for(size_t i = 0; i < objectPoints.size(); ++i) {
    const cv::Point3f& objectPoint = objectPoints.at(i);

    // Rotate and translate
    Eigen::Vector3d P;
    P << objectPoint.x, objectPoint.y, objectPoint.z;

    P = R * P + t;

    Eigen::Vector2d p;
    spaceToPlane(P, p);

    imagePoints.push_back(cv::Point2f(p(0), p(1)));
  }
}

} // namespace camodocal
